#include "ICP.h"
#include <chrono>
#include <stdlib.h>
#define PI 3.14159
int main(int argc, char** argv) {
    if (argc != 2) {
        cout << "usage: main filename\n";
        return 1;
    }
    
    // read data from a file
    string filename = argv[1];
    ifstream fin(filename);
    if (!fin) {
        cout << "the file does not exit!\n";
        return 1;
    }
    
    vector<Point3d> B;
    Point3d temp;
    int line = 1;
    double tempp;
    /*
    for (int i=0; i<num; i++) {
        fin >> temp.x;
        fin >> temp.y;
        fin >> temp.z;
        fin >> tempp;
        fin >> tempp;
        if (line++%2 == 1) B.push_back(temp);
    }
    */
    while (!fin.eof()) {
        fin >> temp.x;
        fin >> temp.y;
        fin >> temp.z;
        fin >> tempp;
        fin >> tempp;
        if (line++%2 == 1) B.push_back(temp);
    }
    fin.close();
    
    Eigen::AngleAxisd rotation_vector(PI/4, Eigen::Vector3d(0, 0, 1));
    Eigen::Matrix3d R_real = rotation_vector.toRotationMatrix();
    Eigen::Vector3d t_real(0.05, 0.05, 0.05);
    
    // create ensemble A
    vector<Point3d> A(B.size());
    rotate_ensemble(B, A, R_real, t_real);
    
    /*
    int lines = A.size();
    cout << "A has " << lines << " points.\n";
    
    double error = 0;
    for (int i=0; i<A.size(); i++) {
        Vector3d b(B[i].x, B[i].y, B[i].z);
        Vector3d a(A[i].x, A[i].y, A[i].z);
        error += ((R_real*b + t_real) - a).norm();
    }
    cout << "error = " << error << endl;
    */
    
    ofstream fout("initialA.dat");
    for (int i=0; i<A.size(); i++) {
        fout << A[i].x << " " << A[i].y << " " << A[i].z << endl;
    }
    fout.close();
    fout.open("initialB.dat");
    for (int i=0; i<B.size(); i++) {
        fout << B[i].x << " " << B[i].y << " " << B[i].z << endl;
    }
    fout.close();
    
    // inilization
    Eigen::Matrix3d R;
    Eigen::Vector3d t;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    /*
    int index = FindClosedPoint(B, A[0]);
    cout << "The cloesest point is " << index << " th point of B\n";
    cout << "the distance is " << distance(A[0], B[index]) << endl;
    */
    pose_estimate_ICP(A, B, R, t);
    
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
    cout << "R = \n";
    cout << R; cout << endl;
    cout << "t = "; cout << t.transpose(); cout << endl;
    fout.open("finalA.dat");
    for (int i=0; i<A.size(); i++) {
        fout << A[i].x << " " << A[i].y << " " << A[i].z << endl;
    }
    fout.close();
    fout.open("finalB.dat");
    for (int i=0; i<B.size(); i++) {
        fout << B[i].x << " " << B[i].y << " " << B[i].z << endl;
    }
    fout.close();
	return 0;
}
